#include "asm/memory.h"
#include "asm/asm-offsets.h"
#include "asm/thread_info.h"
#include "asm/assembler.h"
#include "linux/thread_info.h"

#include "entry-headers.S"

.global stext  				/* 全局标号 */
.global __switch_to
.global c_entry
.global start_kernel
.global preempt_schedule_irq
.global dump_current_cpu_context
.global dump_all_task
.global do_DataAbort
.global cpu_v7_switch_mm
.global processor_id
.global __atags_pointer
.global __machine_arch_type
.global sys_call_table

.equ DISABLE_IRQ, 	0x80
.equ DISABLE_FIQ,  	0x40
.equ SYS_MODE,		0x1f
.equ IRQ_MODE,		0x12
.equ FIQ_MODE,		0x11
.equ SVC_MODE,	  	0x13
.equ ABT_MODE,	  	0x17
.equ UND_MODE,	  	0x1b
.equ MOD_MASK,	 	0x1f


#define KERNEL_RAM_VADDR	(PAGE_OFFSET + TEXT_OFFSET)
#if (KERNEL_RAM_VADDR & 0xffff) != 0x8000
#error KERNEL_RAM_VADDR must start at 0xXXXX8000
#endif

#define PG_DIR_SIZE	0x4000
#define PMD_ORDER	2

	.globl	swapper_pg_dir
	.equ	swapper_pg_dir, KERNEL_RAM_VADDR - PG_DIR_SIZE

stext:
	ldr pc, =Reset_Handler		/* 复位中断 					*/	
	ldr pc, =Undefined_Handler	/* 未定义中断 					*/
	ldr pc, =vector_swi			/* SVC(Supervisor)中断 		*/
	ldr pc, =PrefAbort_Handler	/* 预取终止中断 					*/
	ldr pc, =vector_dabt		/* 数据终止中断 					*/
	ldr	pc, =NotUsed_Handler	/* 未使用中断					*/
	ldr pc, =vector_irq			/* IRQ中断 					*/
	ldr pc, =FIQ_Handler		/* FIQ(快速中断)未定义中断 			*/

/* 复位中断 */	
Reset_Handler:

	cpsid i						/* 关闭全局中断 */

	/* DCache和MMU 
	 * 采取读-改-写的方式。
	 */
	mrc     p15, 0, r0, c1, c0, 0     /* 读取CP15的C1寄存器到R0中       		        	*/
    orr     r0,  r0, #(0x1 << 12)     /* 清除C1寄存器的bit12位(I位)，关闭I Cache            	*/
    bic     r0,  r0, #(0x1 <<  2)     /* 清除C1寄存器的bit2(C位)，关闭D Cache    				*/
    bic     r0,  r0, #0x2             /* 清除C1寄存器的bit1(A位)，关闭对齐						*/
    bic     r0,  r0, #(0x1 << 11)     /* 清除C1寄存器的bit11(Z位)，关闭分支预测					*/
    bic     r0,  r0, #0x1             /* 清除C1寄存器的bit0(M位)，关闭MMU				       	*/
    mcr     p15, 0, r0, c1, c0, 0     /* 将r0寄存器中的值写入到CP15的C1寄存器中	 				*/

	
	/* 进入SVC模式 */
	mrs r0, cpsr
	bic r0, r0, #0x1f 	/* 将r0寄存器中的低5位清零，也就是cpsr的M0~M4 	*/
	orr r0, r0, #0x13 	/* r0或上0x13,表示使用SVC模式					*/
	msr cpsr, r0		/* 将r0 的数据写入到cpsr_c中 					*/

_clear_bss:
	ldr r1, =__bss_start
	ldr r3, =__bss_end
	mov r2, #0x0
1:
	cmp r1, r3
	beq _main
	str r2, [r1], #0x4
	b	1b

_main:
	adr	r3, __mmap_switched_data
	ldmia	r3!, {r4, r5, r6, r7, sp} /* 模仿linux设置prossor_id, machine_arch_type等，并设置sp */

	ldr r1, =stext
	mcr p15, 0, r1, c12, c0, 0  @设置中断向量偏移表
	
	cpsie i				/* 打开全局中断 */
	b start_kernel		/* 跳转到main函数 			 	*/

/* 未定义中断 */
Undefined_Handler:
	msr     CPSR_cxsf, #(DISABLE_FIQ | DISABLE_IRQ | SVC_MODE)
	ldr r0, =Undefined_Handler
	bx r0

/* 预取终止中断 */
PrefAbort_Handler:
	msr     CPSR_cxsf, #(DISABLE_FIQ | DISABLE_IRQ | SVC_MODE)
	ldr r0, =dump_all_task
	blx r0
	b .

/* 未使用的中断 */
NotUsed_Handler:
	msr     CPSR_cxsf, #(DISABLE_FIQ | DISABLE_IRQ | SVC_MODE)
	ldr r0, =NotUsed_Handler
	bx r0

/* FIQ中断 */
FIQ_Handler:
	ldr r0, =FIQ_Handler	
	bx  r0									


vector_swi:
#ifdef NO_DEBUG_MODE
	sub 	sp, sp, #PT_REGS_SIZE
	stmia 	sp, {r0 - r12}
	add 	r8, sp, #S_PC
	stmdb	r8, {sp, lr}^	@ 保存用户模式的sp和lr, 使用 ^ 且基址寄存器不是sp，则表示加载用户模式下的寄存器
	mrs		r8, spsr
	str 	lr, [sp, #S_PC]
	str		r8, [sp, #S_PSR]
	str		r0, [sp, #S_OLD_R0]
#else

	/*
	 * 以上是Linux下原版的vector_swi代码
	 * 因为没有实现unwind, 如果使用sub sp, sp #PT_REGS_SIZE的方式来保存，因为直接对栈指针做了修改,这会导致qemu在单步调试的时候变的很卡
	 * 所以要改用push的方式
	 */

	push 	{r0} 
	
	mrs 	r0, spsr
	push	{r0}

	push 	{lr}
	
	push	{r0 - r1} 			@ skip sp and lr of user-mode
 
	push	{r0 - r12}
	ldr		r0, [sp, #S_OLD_R0]
	str		r0, [sp]
	
	mrs		r7, cpsr
	bic		r7, r7, #MOD_MASK
	orr		r7, r7, #SYS_MODE
	msr		cpsr_cxsf, r7

	mov 	r8,	sp
	mov 	r9, lr
	
	mrs		r7, cpsr
	bic		r7, r7, #MOD_MASK
	orr		r7, r7, #SVC_MODE
	msr		cpsr_cxsf, r7

	str 	r8, [sp, #S_SP]
	str 	r9, [sp, #S_LR]
#endif
	
	ldr		r7, [lr, #-4]
	bic 	r7, r7, #0xff000000		@ 获取 swi-num
	ldr		r8, =sys_call_table
	ldr		r6,	[r8, r7, lsl #2]

	blx		r6

1:	get_thread_info tsk
	b 		ret_fast_syscall

cpu_v7_switch_mm:
	mcr 	p15, 0, r0, c2, c0, 0	

	mov 	r0, #0
	mcr 	p15, 0, r0, c8, c7, 0

	dsb
	isb
	mov 	pc, lr

__irq_invalid:
	mrs r0, spsr
	b .

ret_to_user_from_irq:
	ldr	r1, [tsk, #TI_FLAGS]
	tst	r1, #_TIF_WORK_MASK
	bne	slow_work_pending
no_work_pending:
	b restore_user_regs

slow_work_pending:
	mov	r0, sp				@ 'regs'
	mov	r2, why				@ 'syscall'
	bl	do_work_pending
	cmp	r0, #0
	beq	no_work_pending
	b . 					@ something wrong


__irq_usr:
	sub		sp, sp, #PT_REGS_SIZE
 	stmib	sp, {r1 - r12}
	ldmia	r0, {r3 - r5}
	add		r0, sp, #S_PC		
	
	mov		r6, #-1				

	str		r3, [sp]			
	stmia	r0, {r4 - r6}
 	stmdb	r0, {sp, lr}^ 			@ ^ 的意思为把user(system)模式下的sp,lr保存到r0指向的栈中

	ldr		r1, =handle_arch_irq 	@ 展开宏irq_handler
	ldr 	r1, [r1]
	mov		r0, sp
	blx 	r1
	
	get_thread_info tsk
	mov	why, #0

	b ret_to_user_from_irq

/*
 * 为方便调试和阅读，svc_exit和svc_entry等代码代码都已经展开，为了不影响阅读，这里注释掉
 */
/*

irq_handler:
	ldr	r1, =handle_arch_irq
	mov	r0, sp
	ldr	pc, [r1]

usr_entry:
	sub	sp, sp, #PT_REGS_SIZE
 	stmib	sp, {r1 - r12}
	ldmia	r0, {r3 - r5}
	add	r0, sp, #S_PC		
	mov	r6, #-1				

	str	r3, [sp]			
	
	stmia	r0, {r4 - r6}
 	stmdb	r0, {sp, lr}^

svc_exit:
	msr	spsr_cxsf, r5
	ldmia	sp, {r0 - pc}^			

svc_entry:
	sub	sp, sp, #(SVC_REGS_SIZE - 4)
	stmia	sp, {r1 - r12}

	ldmia	r0, {r3 - r5}
	add	r7, sp, #S_SP - 4	
	mov	r6, #-1				
	add	r2, sp, #(SVC_REGS_SIZE - 4)
	str	r3, [sp, #-4]!		

	mov	r3, lr

	stmia	r7, {r2 - r6}

	@get_thread_info tsk
	@ldr	r0, [tsk, #TI_ADDR_LIMIT]
	@mov	r1, #TASK_SIZE
	@str	r1, [tsk, #TI_ADDR_LIMIT]
	@str	r0, [sp, #SVC_ADDR_LIMIT]

v7_early_abort:
	mrc	p15, 0, r1, c5, c0, 0		@ get FSR
	mrc	p15, 0, r0, c6, c0, 0		@ get FAR

	blx	do_DataAbort
	mov pc, lr

*/

__irq_svc:
	sub		sp, sp, #(SVC_REGS_SIZE - 4)	@ 展开宏 svc_entry，方便调试
	stmia	sp, {r1 - r12}					@ 保存r1-r12

	ldmia	r0, {r3 - r5}					@ 出栈r0, lr, cpsr
	add		r7, sp, #S_SP - 4				@ r7指向sp的S_SP位置
	mov		r6, #-1							@
	add		r2, sp, #(SVC_REGS_SIZE - 4)	@ r2指向sp初始位置
	str		r3, [sp, #-4]!					@ 保存r0
	
	mov		r3, lr							@ r3保存lr
	stmia	r7, {r2 - r6}					@ 保存sp, lr, pc, cpsr, orig_R0
	
	ldr		r1, =handle_arch_irq 			@ 展开宏irq_handler
	ldr 	r1, [r1]
	mov		r0, sp
	blx 	r1
	
	get_thread_info  tsk
	
	ldr 	r8, [tsk, #TI_PREEMPT]
	ldr 	r0, [tsk, #TI_FLAGS]	@ 获取flags
	teq 	r8, #0					@ 判断preempt_count是否为0
	movne 	r0, #0 					@ 如果不为0,表示不可抢占,将flags清0
	tst 	r0, #_TIF_NEED_RESCHED 	@ 判断是否需要调度
	blne 	svc_preempt 			@ 如果需要调度,则进行调度

	msr		spsr_cxsf, r5   		@ 展开宏 svc_exit，方便调试
	ldmia	sp, {r0 - pc}^			@ load r0 - pc, cpsr

svc_preempt:
	mov r8, lr
1:  bl preempt_schedule_irq
	ldr r0, [tsk, #TI_FLAGS]	
	tst r0, #_TIF_NEED_RESCHED
	bxeq r8
	b 1b

vector_irq:
	cpsid 	f
	sub		lr, lr, #4						@ 调整lr

	stmia	sp, {r0, lr}					@ 保存r0,lr
	mrs		lr, spsr						@ 获取spsr
	str		lr, [sp, #8]					@ 保存spsr

	mrs		r0, cpsr
	eor		r0, r0, #(IRQ_MODE ^ SVC_MODE)
	msr		spsr_cxsf, r0					@ 准备切换到svc模式

	and		lr, lr, #0x0f					@ 获取异常类型,cpsr寄存器M字段的后4位，即cpsr中的3 - 0位
	mov		r0, sp							@ 传递sp参数
 	ldr		lr, [pc, lr, lsl #2]			@ 根据异常类型跳转到对应的处理函数(后4位*4为偏移地址，加上当前PC指针就可拿到具体函数地址)
	movs	pc, lr				

	.long	__irq_usr				@  0  (USR_26 / USR_32) M: 10000
	.long	__irq_invalid			@  1  (FIQ_26 / FIQ_32) M: 10001
	.long	__irq_invalid			@  2  (IRQ_26 / IRQ_32) M: 10010
	.long	__irq_svc				@  3  (SVC_26 / SVC_32) M: 10011
	.long	__irq_invalid			@  4
	.long	__irq_invalid			@  5
	.long	__irq_invalid			@  6
	.long	__irq_invalid			@  7
	.long	__irq_invalid			@  8
	.long	__irq_invalid			@  9
	.long	__irq_invalid			@  a
	.long	__irq_invalid			@  b
	.long	__irq_invalid			@  c
	.long	__irq_invalid			@  d
	.long	__irq_invalid			@  e
	.long	__irq_usr				@  f  (SYSTEM) M: 11111


ret_from_exception:
	get_thread_info tsk
	mov	why, #0
	b .

/*
 * data abort usr和svc的实现都不完整
 */
__dabt_usr:
	sub		sp, sp, #PT_REGS_SIZE
 	stmib	sp, {r1 - r12}
	ldmia	r0, {r3 - r5}
	add		r0, sp, #S_PC		
	
	mov		r6, #-1				

	str		r3, [sp]			
	stmia	r0, {r4 - r6}
 	stmdb	r0, {sp, lr}^ 				@ ^ 的意思为把user(system)模式下的sp,lr保存到r0指向的栈中
	
	mov 	r2, sp
	//bl 	v7_early_abort
	mrc		p15, 0, r1, c5, c0, 0		@ get FSR
	mrc		p15, 0, r0, c6, c0, 0		@ get FAR
	
	ldr		r3, =do_DataAbort
	blx 	r3
	
	b		ret_from_exception

__dabt_svc:
	sub		sp, sp, #(SVC_REGS_SIZE - 4)	@ 展开宏 svc_entry，方便调试
	stmia	sp, {r1 - r12}					@ 保存r1-r12

	ldmia	r0, {r3 - r5}					@ 出栈r0, lr, cpsr
	add		r7, sp, #S_SP - 4				@ r7指向sp的S_SP位置
	mov		r6, #-1							@
	add		r2, sp, #(SVC_REGS_SIZE - 4)	@ r2指向sp初始位置
	str		r3, [sp, #-4]!					@ 保存r0
	
	mov		r3, lr							@ r3保存lr
	stmia	r7, {r2 - r6}					@ 保存sp, lr, pc, cpsr, orig_R0
	
	mov r2, sp
	//bl v7_early_abort
	mrc	p15, 0, r1, c5, c0, 0		@ get FSR
	mrc	p15, 0, r0, c6, c0, 0		@ get FAR
	
	ldr	r3, =do_DataAbort
	blx r3

	
	msr	spsr_cxsf, r5   		@ 展开宏 svc_exit，方便调试
	ldmia	sp, {r0 - pc}^		@ load r0 - pc, cpsr

vector_dabt:
	cpsid f
	sub	lr, lr, #8						@ 调整lr

	stmia	sp, {r0, lr}				@ 保存r0,lr
	mrs	lr, spsr						@ 获取spsr
	str	lr, [sp, #8]					@ 保存spsr

	mrs	r0, cpsr
	eor	r0, r0, #(ABT_MODE ^ SVC_MODE)
	msr	spsr_cxsf, r0					@ 准备切换到svc模式

	and	lr, lr, #0x0f					@ 获取异常类型	
	mov	r0, sp							@ 传递sp参数
 	ldr	lr, [pc, lr, lsl #2]			@ 根据异常类型跳转到对应的处理函数
	movs	pc, lr				

	.long	__dabt_usr				@  0  (USR_26 / USR_32)
	.long	__irq_invalid			@  1  (FIQ_26 / FIQ_32)
	.long	__irq_invalid			@  2  (IRQ_26 / IRQ_32)
	.long	__dabt_svc				@  3  (SVC_26 / SVC_32)
	.long	__irq_invalid			@  4
	.long	__irq_invalid			@  5
	.long	__irq_invalid			@  6
	.long	__irq_invalid			@  7
	.long	__irq_invalid			@  8
	.long	__irq_invalid			@  9
	.long	__irq_invalid			@  a
	.long	__irq_invalid			@  b
	.long	__irq_invalid			@  c
	.long	__irq_invalid			@  d
	.long	__irq_invalid			@  e
	.long	__dabt_usr				@  f

__switch_to:		
	add 	ip, r1, #TI_CPU_SAVE
	stmia 	ip!, {r4 - sl, fp, sp, lr}

	add 	r4, r2, #TI_CPU_SAVE
	ldmia 	r4, {r4 - sl, fp, sp, pc}


	.align	2
	.type	__mmap_switched_data, %object
__mmap_switched_data:
	.long	processor_id			
	.long	__machine_arch_type		
	.long	__atags_pointer			
	.long	0				
	.long	init_thread_union + THREAD_START_SP 
	.size	__mmap_switched_data, . - __mmap_switched_data